#include "algoritmos.h"
#include <math.h>
double sumar(vector<float> x)
{
    double sum = 0;
    for(unsigned int i = 0; i < x.size(); i++)
    {
        sum += x[i];
    }
    return sum;
}


float sumaNueva(int desde, Matriz& A, vector<float>& x)
{
    float sum = 0;
    if(desde < sqrt(x.size())){
        for(unsigned int i = 1; i < desde  + sqrt(x.size()); i++)
        {
            sum += x[x.size()-i] * A.get(desde, A.size()-i+1);
        }
    }else{
        for(unsigned int i = 1; i < x.size(); i++)
        {
            sum += x[x.size()-i] * A.get(desde, A.size()-i+1);
        }
    }
    return sum;
}
vector<float> resMatrizTriangular(Matriz& m, vector<float>& b)
{

    float valorX;
    //int ancho = floor(sqrt(m.size()));
    int n = m.size();
    vector<float> x(n);   // vector resultado
    x[n-1]=b[n-1]/m.get(n,n);

    for(int k = n; k > 1; k--)      // calculamos el vector x con el metodo de matriz triangular
    {
		valorX = (b[k-1] - sumaNueva(k, m, x))/m.get(k,k);
		x[k-1] = valorX;
    }
    return x;
}


double PSNR(Foto original, Foto filtrada)
{
    long N = original.size()*original.size();
    long sum = 0;
    for(int i=0 ; i<(int)original.size(); i++)
    {
        for(int j=0 ; j<(int)original.size(); j++)
        {
            sum+=pow(original[i][j]-filtrada[i][j],2);
        }
    }

    double ECM =  (1/(double)N)* (double) sum;

    return (10*log10(255*255/ECM));

}


vector<float> gauss(Matriz& m, vector<float>& b)
{

    int n = m.size();
//    char basu;
//cout << "n: " << n;
    for(int k = 1; k<= n; k++)      // k = iteraciones
    {

//        cout << " Iteracion: " << k << endl;
//        cin >> basu;

		if(k < n - sqrt(n)){
			int cont = 1;

			for(int i = k+1; i <= k + floor(sqrt(n)); i++)     // i = fila
			{
				float mik = m.get(i,k);
				if(k + cont < n - sqrt(n)){
                    for(int j = k; j <= k + floor(sqrt(n)) + cont; j++)     // j = columna
                    {
                        float valor = m.get(i,j)-(m.get(k,j)*mik/m.get(k,k));
//                        cout << "M(" << i << ", " << j << " ) = " << m.get(i,j);
//                        cout << " - ";
//                        cout << "M(" << k << ", " << j << ") = " << m.get(k,j);
//                        cout << " * ";
//                        cout << "M(" << i << ", " << k << ") = " << mik;
//                        cout << " / ";
//                        cout << "M(" << k << ", " << k << ") = " << m.get(k,k);
//                        cout << endl;
//                        cout << "M(" << i << ", " << j << ") = VALOR = " << valor;
//                        cout << endl;
                        m.set(i,j,valor);    // voy modificando la matriz dejando en 0 la triangular inferior

                    }
                    cont++;
                    b[i-1] = b[i-1] - b[k-1]*mik/m.get(k,k);
                }else{
                    for(int j = k; j <= n; j++)     // j = columna
                    {
                        float valor = m.get(i,j)-(m.get(k,j)*mik/m.get(k,k));
//                        cout << "M(" << i << ", " << j << " ) = " << m.get(i,j);
//                        cout << " - ";
//                        cout << "M(" << k << ", " << j << ") = " << m.get(k,j);
//                        cout << " * ";
//                        cout << "M(" << i << ", " << k << ") = " << mik;
//                        cout << " / ";
//                        cout << "M(" << k << ", " << k << ") = " << m.get(k,k);
//                        cout << endl;
//                        cout << "M(" << i << ", " << j << ") = VALOR = " << valor;
//                        cout << endl;
                        m.set(i,j,valor);    // voy modificando la matriz dejando en 0 la triangular inferior

                    }
                    cont++;
                    b[i-1] = b[i-1] - b[k-1]*mik/m.get(k,k);

                }

			}

		}else{
			for(int i = k+1; i <= n; i++)     // i = fila
			{

				float mik = m.get(i,k);
				for(int j = k; j <= n; j++)     // j = columna
				{
					float valor = m.get(i,j)-m.get(k,j)*mik/m.get(k,k);
//					cout << "M(" << i << ", " << j << " ) = " << m.get(i,j);
//					cout << " - ";
//					cout << "M(" << k << ", " << j << ") = " << m.get(k,j);
//					cout << " * ";
//					cout << "M(" << i << ", " << k << ") = " << mik;
//					cout << " / ";
//					cout << "M(" << k << ", " << k << ") = " << m.get(k,k);
//					cout << endl;
//					cout << "M(" << i << ", " << j << ") = VALOR = " << valor;
//					cout << endl;
        			m.set(i,j,valor);    // voy modificando la matriz dejando en 0 la triangular inferior
				}

				b[i-1] = b[i-1] - b[k-1]*mik/m.get(k,k);
			}
		}

	}
    // Ahora m es triangular superior.
    cout << "TERMINO LA TRIANGULACION" << endl;

    vector<float> x = resMatrizTriangular(m,b);   // vector resultado

    cout << "TERMINO EL DESPEJE DE X" << endl;
    return x;
}
